#include <main.hpp>
#include <chrono>  // 添加计时器所需的头文件

// IMG
unsigned short thre = 120;

int lastDuty = servo_angle_mid;
int servoOutput = 0;
int servoDutyCycle = servo_angle_mid;
int motorL = 0;
int motorR = 0;

int flag_left = 0, flag_right = 0;
bool specialState = false;  // 用于标识是否处于特殊状态
auto specialStateStartTime = chrono::high_resolution_clock::now();  // 记录特殊状态的开始时间
float accumulatedAngle = 0;  // 用于累计陀螺仪角度

// void callback(){
//     Gyroscope_GetData();        
//     // Get_Gyroscope_Angle();   // 角度积分
// }
void ttt(float& angle, int& WC, int& chujie, int& zerbra);

int main()
{
    Init_dev();

    /**********************************  camera   *******************************/
    VideoCapture cap(0);
    if (!cap.isOpened())
    {
        cerr << "Failed to open camera!" << endl;
    }

    cap.set(CAP_PROP_FOURCC, VideoWriter::fourcc('M', 'J', 'P', 'G'));
    cap.set(CAP_PROP_FRAME_WIDTH, 160);
    cap.set(CAP_PROP_FRAME_HEIGHT, 120);
    cap.set(CAP_PROP_FPS, 120);

    int sockfd = socket(AF_INET, SOCK_DGRAM, 0);

    if (sockfd < 0)
    {
        cerr << "Failed to create socket!" << endl;
    }

    // 配置目标地址
    struct sockaddr_in dest_addr;
    memset(&dest_addr, 0, sizeof(dest_addr));
    dest_addr.sin_family = AF_INET;
    dest_addr.sin_port = htons(UDP_PORT);

    if (inet_pton(AF_INET, DST_IP, &dest_addr.sin_addr) <= 0)
    {
        cerr << "Invalid IP address!" << endl;
        close(sockfd);
    }
    vector<int> encode_params = {IMWRITE_JPEG_QUALITY, 80};

    Mat frame;
    auto start = chrono::high_resolution_clock::now();
    int frameCount = 0;

    // Zero_Point_Detect();

    // pit_ms_init(10,callback);

    while (1)
    {
        if (!cap.read(frame) || frame.empty())
        {
            cerr << "Failed to capture frame!" << endl;
            continue;
        }

        // Get_Gyroscope_Angle();
        // cout << "Z_Gyro : " << FJ_Angle << endl;
        // Clear_Gyroscope_Angle();
        // frameCount++;
        // auto end = chrono::high_resolution_clock::now();
        // chrono::duration<double> elapsed = end - start;

        // double fps = frameCount / elapsed.count();

        // cout << "FPS: " << fps << endl;

        /*******************************  processImage  ***************************/

        float angle;
        int chujie, zerbra, WC;
        Mat binary = binary_img(frame, thre);
        Mat processedFrame = processImage(binary, angle, WC, chujie, zerbra, flag_left, flag_right);
        if (processedFrame.empty())
        {
            continue;
        }
        // if (!sendProcessedImageViaUDP(processedFrame, sockfd, dest_addr, encode_params))
        // {
        //     cerr << "Failed to send processed image via UDP." << endl;
        // }
        ttt(angle, WC, chujie, zerbra);
    }

    cap.release();
    LMotor_value(0);
    RMotor_value(0);
    Servo_value(150000);

    return 0;
}

void ttt(float& angle, int& WC, int& chujie, int& zerbra)
{
    // output = kp,kp_fang,kd,kd,ki;
    erciPD pdf(3, 0.03, 1.4, 0, 0); //  弯道
    erciPD pdm(2.8, 0.014, 0.3, 0, 0);
    erciPD pdl(2.6, 0, 0.1, 0, 0); //  直道

    incrPIDL pi(700, 20, 10);
    incrPIDR piR(480, 20, 10);

    int motor_goal = 64;

    int motor_low = 42;
    int motor_www = 48;
    int motor_yyy = 52;
    int motor_mid = 56;
    int motor_high = 64;

    float LSpeed = -R_encoder_value();

    if (chujie == 1)
    {
        Servo_value(150000);
        LMotor_value(15000);
        RMotor_value(15000);
    }
    else
    {
        if (zerbra > 110)
        {
            Servo_value(150000);
            LMotor_value(30000);
            RMotor_value(30000);
        }
        // else if (flag_left == 1 && flag_right == 0 && !specialState)
        // {
        //     // 进入特殊状态
        //     specialState = true;
        //     specialStateStartTime = chrono::high_resolution_clock::now();
        //     Servo_value(150000);
        //     LMotor_value(30000);
        //     RMotor_value(30000);
        // }
        // else if (specialState)
        // {
        //     auto now = chrono::high_resolution_clock::now();
        //     chrono::duration<double> specialStateElapsed = now - specialStateStartTime;
        //     if (specialStateElapsed.count() > 0.5)
        //         specialState = false;
        //         Servo_value(164000);
        //         // 开始陀螺仪角度积分计算
        //         // accumulatedAngle = 0;
        //         // accumulatedAngle += Get_Gyroscope_Angle();  // 假设Get_Gyroscope_Angle()返回当前陀螺仪角度
        //         while (accumulatedAngle < 180)
        //         {
        //             // Gyroscope_GetData();  // 获取陀螺仪数据
        //             // accumulatedAngle += Get_Gyroscope_Angle();  // 累计角度
        //             // cout << "Accumulated Angle: " << accumulatedAngle << endl;  // 输出累计角度
        //         }
        // }
        else
        {
            if (WC < 100)
            {
                servoOutput = pdf.update(angle_goal, angle, 0);
                servoDutyCycle = (servo_angle_mid + servoOutput * 100) * 0.98 + 0.02 * lastDuty;
                if (angle > 7)
                {
                    motorL = 21000;
                    motorR = 14000;
                }
                else if (angle < -7)
                {
                    motorL = 14000;
                    motorR = 21000;
                }
                else
                {
                    motorL = pi.update(motor_low, LSpeed);
                    motorR = motorL;
                }
            }
            else if (WC < 106)
            {
                servoOutput = pdm.update(angle_goal, angle, 0);
                servoDutyCycle = (servo_angle_mid + servoOutput * 100) * 0.96 + 0.04 * lastDuty;
                motorL = pi.update(motor_low, LSpeed);
                motorR = motorL;
            }
            else if (WC < 110)
            {
                servoOutput = pdm.update(angle_goal, angle, 0);
                servoDutyCycle = (servo_angle_mid + servoOutput * 100) * 0.94 + 0.04 * lastDuty;
                motorL = piR.update(motor_www, LSpeed);
                motorR = motorL;
            }
            else if (WC < 113)
            {
                servoOutput = pdl.update(angle_goal, angle, 0);
                servoDutyCycle = (servo_angle_mid + servoOutput * 100) * 0.92 + 0.08 * lastDuty;
                motorL = piR.update(motor_yyy, LSpeed);
                motorR = motorL;
            }
            else if (WC < 116)
            {
                servoOutput = pdl.update(angle_goal, angle, 0);
                servoDutyCycle = (servo_angle_mid + servoOutput * 100) * 0.9 + 0.1 * lastDuty;
                motorL = piR.update(motor_mid, LSpeed);
                motorR = motorL;
            }
            else
            {
                servoOutput = pdl.update(angle_goal, angle, 0);
                servoDutyCycle = (servo_angle_mid + servoOutput * 100) * 0.88 + 0.12 * lastDuty;
                motorL = piR.update(motor_high, LSpeed);
                motorR = motorL;
            }
        }
    }

    Servo_value(servoDutyCycle);
    // cout << "servoDutyCycle: " << servoDutyCycle << endl;
    // cout << "angle: " << angle << endl;
    LMotor_value(motorL);
    RMotor_value(motorR);

    lastDuty = servoDutyCycle;

    // cout << "motorL: " << motorL << "   motorR: " << motorR << endl;
    // cout << "L_encoder_value: " << LSpeed << endl;
    // cout << " WC" << WC << endl;

    // 电机方向（不动）
    LMotor_dir();
    RMotor_dir();
}
